/**
 * @file motor.c
 * @brief 大疆电机驱动模块
 * @details 包含电机反馈数据解析、CAN通信初始化及电机控制初始化功能
 *          支持gm6020（新旧版）、c610、c620等型号电机
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 * | 版本 | 修改日期   | 修改人       | 修改内容                 |
 * |------|------------|--------------|--------------------------|
 * | 1.0  | 2025-08-08 | WeiFan Qiu   | 初始版本，实现基础驱动逻辑 |
 * 
 * @bug 待解决问题:
 * - 尚未加入达妙电机相关的
 */
// 包含电机驱动核心头文件
#include "motor.h"
// 包含通用功能库（滤波、数学运算等）
#include "QWF_Include.h"
// 包含自定义控制逻辑（如底盘/云台控制）
#include "QWF_Custom_Ctrl.h"
// 包含CAN通信接口
#include "QWF_Can.h"

/**
 * @brief 解算大疆电机数据(不计圈数)
 * @param motor 电机结构体指针，存储电机状态及配置
 * @param Data CAN总线接收的原始数据（8字节）
 * @note  数据格式：[0-1]角度(0-8191)，[2-3]转速，[4-5]扭矩，[6]温度
 * @note  只是定义该函数，实际使用的是最下面的函数数组
 */
static void DJI_Motor_return_without_round(DJI_Motor_t* motor,uint8_t* Data)
{
    // 解析角度值（减去初始偏移量）
    motor->rx_meas.angle[2] = motor->rx_meas.angle[last] = motor->rx_meas.angle[now] = (int16_t)((Data[0]<<8)|Data[1]) - motor->rx_meas.angle_begin;
    motor->rx_meas.rpm      = (int16_t)((Data[2]<<8)|Data[3]);  // 解析转速（单位：rpm）
    motor->rx_meas.torque   = (int16_t)((Data[4]<<8)|Data[5]);  // 解析扭矩电流
    motor->rx_meas.trm     = Data[6];                           // 解析温度（单位：℃）
    motor->rx_meas.round   = 0;                                 // 无圈数统计时置0
}

/**
 * @brief 解算大疆电机数据(计圈数过零)
 * @param motor 电机结构体指针，存储电机状态及配置
 * @param Data CAN总线接收的原始数据（8字节）
 * @note  通过比较前后角度差判断圈数变化（每圈8191单位）
 * @note  只是定义该函数，实际使用的是最下面的函数数组
 * @bug	  如果电机信号不稳可能出现一直过零导致圈数一直增加的情况
 */
static void DJI_Motor_return_with_round(DJI_Motor_t* motor,uint8_t* Data)
{
    motor->rx_meas.angle[last] = motor->rx_meas.angle[now];  // 保存上一周期角度值
    motor->rx_meas.angle[now]  = (int16_t)((Data[0]<<8)|Data[1]) - motor->rx_meas.angle_begin;  // 当前角度值
    motor->rx_meas.rpm         = (int16_t)((Data[2]<<8)|Data[3]);  // 转速
    motor->rx_meas.torque      = (int16_t)((Data[4]<<8)|Data[5]);  // 扭矩
    motor->rx_meas.trm        = Data[6];                          // 温度

    // 圈数计算逻辑：角度突变瞬间±4096时调整圈数（超过半圈视为跨圈）
    if(motor->rx_meas.angle[last] - motor->rx_meas.angle[now] >= 4096) {
        motor->rx_meas.round++;
    } else if(motor->rx_meas.angle[last] - motor->rx_meas.angle[now] <= -4096) {
        motor->rx_meas.round--;
    }
    // 总角度=当前角度+圈数×单圈单位（8191）
    motor->rx_meas.angle[2] = motor->rx_meas.angle[now] + motor->rx_meas.round * 8191;
}

/**
 * @brief 初始化大疆电机CAN通信参数
 * @param[in] DJI_Motor 待初始化的电机结构体指针
 * @details 根据电机类型（gm6020/c620等）和CAN总线号（1/2/3）配置：
 *          - 发送ID（StdId）及数据存储地址
 *          - 接收ID（StdId）及使能接收标志
 * @attention 只在程序开始初始化一次
 * @note	未配置的电机没不会初始化，即为0，也就是DJI_Motor->config.type==undef_motor
 */
static void DJI_Motor_Init(DJI_Motor_t* DJI_Motor)
{
    static QWF_can_obj_t* motor_can_obj = NULL;  // CAN总线对象指针

    // 根据配置的CAN总线号获取对应对象
    switch(DJI_Motor->config.can) {
        case 1: motor_can_obj = &get_qwf_can_obj(1); break;
        case 2: motor_can_obj = &get_qwf_can_obj(2); break;
        case 3: motor_can_obj = &get_qwf_can_obj(3); break;
        default: return;  // 无效总线号直接返回
    }

    // 根据电机类型配置通信参数
    switch((int)DJI_Motor->config.type) {
        /* gm6020 新版（协议ID 0x1fe/0x2fe） */
        case DJI_gm6020_new_i: {
            if(DJI_Motor->config.mode != Only_rx) {  // 非仅接收模式
                if(DJI_Motor->config.ID <= 4) {  // 1-4号电机使用0x1fe发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id1fe[2*DJI_Motor->config.ID-2];
                    DJI_Motor->config.tx_stdId = 0x1fe;
                    motor_can_obj->tx.StdId_use.DJI.id1fe = 1;  // 标记ID已使用
                } else if(DJI_Motor->config.ID <= 8) {  // 5-8号电机使用0x2fe发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id2fe[2*(DJI_Motor->config.ID-4)-2];
                    DJI_Motor->config.tx_stdId = 0x2fe;
                    motor_can_obj->tx.StdId_use.DJI.id2fe = 1;
                }
            }
            if(DJI_Motor->config.mode != Only_tx) {  // 非仅发送模式
                motor_can_obj->rx.StdId_use.DJI.id204[DJI_Motor->config.ID-1] = 1;  // 使能0x204+ID接收
                DJI_Motor->config.rx_stdId = 0x204 + DJI_Motor->config.ID;
            }
        } break;

        /* gm6020 旧版（协议ID 0x1ff/0x2ff） */
        case DJI_gm6020_old: {
            if(DJI_Motor->config.mode != Only_rx) {
                if(DJI_Motor->config.ID <= 4) {  // 1-4号电机使用0x1ff发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id1ff[2*DJI_Motor->config.ID-2];
                    motor_can_obj->tx.StdId_use.DJI.id1ff = 1;
                    DJI_Motor->config.tx_stdId = 0x1ff;
                } else if(DJI_Motor->config.ID <= 8) {  // 5-8号电机使用0x2ff发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id2ff[2*(DJI_Motor->config.ID-4)-2];
                    motor_can_obj->tx.StdId_use.DJI.id2ff = 1;
                    DJI_Motor->config.tx_stdId = 0x2ff;
                }
            }
            if(DJI_Motor->config.mode != Only_tx) {
                motor_can_obj->rx.StdId_use.DJI.id204[DJI_Motor->config.ID-1] = 1;
                DJI_Motor->config.rx_stdId = 0x204 + DJI_Motor->config.ID;
            }
        } break;
        /* c620电机（协议ID 0x200/0x1ff） */
        case DJI_c620: {
            if(DJI_Motor->config.mode != Only_rx) {
                if(DJI_Motor->config.ID <= 4) {  // 1-4号电机使用0x200发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id200[2*DJI_Motor->config.ID-2];
                    DJI_Motor->config.tx_stdId = 0x200;
                    motor_can_obj->tx.StdId_use.DJI.id200 = 1;
                } else if(DJI_Motor->config.ID <= 8) {  // 5-8号电机使用0x1ff发送ID
                    DJI_Motor->config.tx_adress = &motor_can_obj->tx.data.DJI.id1ff[2*(DJI_Motor->config.ID-4)-2];
                    motor_can_obj->tx.StdId_use.DJI.id1ff = 1;
                    DJI_Motor->config.tx_stdId = 0x1ff;
                }
            }
            if(DJI_Motor->config.mode != Only_tx) {
                motor_can_obj->rx.StdId_use.DJI.id200[DJI_Motor->config.ID-1] = 1;
                DJI_Motor->config.rx_stdId = 0x200 + DJI_Motor->config.ID;
            }
        } break;

        default: break;
    }
}

/**
 * @brief 全局电机初始化函数（根据电路板类型差异化初始化）
 * @details 条件编译区分云台板和底盘板：
 *          - 云台	初始化pitch/yaw/拨弹电机
 *          - 底盘	初始化转向/轮边电机
 */
void Motor_Init(void){
	/*------------------云台电机------------------*/
    #if Circuit_Boards != Chassis_Circuit_Boards  
        #include "head.h"
        // 初始化云台电机（pitch轴、yaw轴、左右拨弹、拨轮）
        DJI_Motor_Init(&head->part.gimbal.pitch.motor);
        DJI_Motor_Init(&head->part.gimbal.yaw.motor);
        DJI_Motor_Init(&head->part.shoot.fire.l.motor);
        DJI_Motor_Init(&head->part.shoot.fire.r.motor);
        DJI_Motor_Init(&head->part.shoot.knob.motor);
    #endif
	/*------------------底盘电机------------------*/
    #if Circuit_Boards != Head_Circuit_Boards  
        #include "chassis.h"
        // 初始化底盘电机（yaw轴、前后左右轮边/转向）
        DJI_Motor_Init(&chassis->part.yaw.motor);
		#if Chassis_Wheel_Type==Helf_Helm_Wheel
        DJI_Motor_Init(&chassis->part.f.helm.motor);
        DJI_Motor_Init(&chassis->part.f.wheel.motor);
        DJI_Motor_Init(&chassis->part.b.helm.motor);
        DJI_Motor_Init(&chassis->part.b.wheel.motor);
        DJI_Motor_Init(&chassis->part.l.wheel.motor);
        DJI_Motor_Init(&chassis->part.r.wheel.motor);
		#elif Chassis_Wheel_Type==Leg_Wheel
		
		#endif
    #endif
    return;
}

/**
 * @brief 电机反馈处理函数指针数组
 * @param motor 电机结构体指针，存储电机状态及配置
 * @param Data CAN总线接收的原始数据（8字节）
 * @details 根据模式选择无圈数/带圈数处理函数
 *          - 索引0：无圈数模式
 *          - 索引1：带圈数模式
 * @example	不记录圈数	DJI_Motor_return[without_round]			(&chassis->part.l.wheel.motor			,get_qwf_can_data_rx(1)	);
 * @example	记录圈数	DJI_Motor_return[without_round]			(&head->part.gimbal.pitch.motor			,get_qwf_can_data_rx(1)	);
 */
DJI_Motor_Return_Function  DJI_Motor_return[motor_return_mode_num] = {DJI_Motor_return_without_round, DJI_Motor_return_with_round};
